from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.substitutions import FindPackageShare
import os
import math

def generate_launch_description():

    return LaunchDescription([

        DeclareLaunchArgument(
            'ugv_ids',
            default_value="['ugv1']",
            description='List of UGV IDs to bridge, e.g. ["ugv1", "ugv2"]'
        ),

        Node(package='sls_launch_sim',
            executable='position_publisher',
            name='position_publisher',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),

        Node(
            package='sls_launch_sim',
            executable='relay_node',
            name='relay_node',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='oakd_tf_pub',
            arguments=[
                '0.79', '0.027', '0.35',         # translation: x y z
                '0', '0', '0',         # rotation: roll pitch yaw
                'base_link',           # parent frame
                'rover_ackermann/OakD-Lite/base_link/StereoOV7251'  # child frame (from your pointcloud frame_id)
            ],
            output='screen'
        ),
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='oakd_rgb_tf_pub',
            arguments=[
                '0.79', '0.027', '0.35',         # translation: x y z（从模型 pose 设置复制）
                '0', '0', '0',                   # rotation: roll pitch yaw（单位为弧度）
                'base_link',                    # parent frame（一般为机器人底盘）
                'rover_ackermann/OakD-Lite/base_link/IMX214'  # child frame（相机的 frame_id）
            ],
            output='screen'
        ),
        Node(
            package='sls_launch_sim',
            executable='chassis_cmd_node',
            name='chassis_cmd_node',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),
        Node(
            package='sls_launch_sim',
            executable='motor_state_bridge',
            name='motor_state_bridge',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),
        Node(
            package='sls_launch_sim',
            executable='steering_bridge',
            name='steering_bridge',
            output='screen',
            parameters=[{'ugv_ids': LaunchConfiguration('ugv_ids')}]
        ),
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='lidar_tf_pub',
            arguments=['0', '0', '1.48', '0', '0', '0', 'base_link', 'rover_ackermann/lidar_link/lidar_sensor'],
            output='screen',
        ),

    ])
